#include <Servo.h> //biblioteka zarządzająca servami
#include <math.h>
#define PI 3.14159265 //definiujemy stałą Pi potrzebną do obliczeń
#include <AccelStepper.h> //biblioteka zarządzająca silnikiem krokowym
#define dirPin 2
#define stepPin 3
Servo S1;
Servo S2;
Servo S3;
int S1max=180,S1min=23;//maksymalny i minimalny kąt 1 serva
int S2max=180,S2min=0;//maksymalny i minimalny kąt 2 serva 
int a=179,b=145,c=53,e=90;//wymiary w mm prototypu
long czas,dystans;
int dlugosc=0, wysokosc=0, kacik=0;
bool z=false, v=false, d=false;
int f=0;
float przeciw;
int wysoko=-50;
float prze=-50;
int kroki;
void check(float,float,float);//funkcja liczy kąty jakie potrzebujem dla serva1 i serva2
void idzie(float,float,int);//porusza serva 1 i 2 w odpowiedniej kolejności i szybkości
void wraca(float,float,int);//porusza serva 1 i 2 w odpowiedniej szybkości i odwrotnej kolejności
void kroki1(float,float);//oblicza kąt oraz dł potrzebną do funkcji check obraca silnik krokowy o zadany kąt

AccelStepper stepper = AccelStepper(1, stepPin, dirPin);

void setup() {
   delay(3000);
  Serial.begin(9600);
  pinMode(A0,INPUT_PULLUP);
  S1.attach(9);
  S2.attach(10);
  S3.attach(11);
  S2.write(90);
  S1.write(100);
  delay(10);
  pinMode(12,OUTPUT);
  pinMode(stepPin, OUTPUT);
  pinMode(dirPin, OUTPUT);
  pinMode(13,OUTPUT); // zasilanie silnika
  stepper.setMaxSpeed(1000);
  pinMode(4,OUTPUT); //stały sygnał dla 1/4 kroku
  digitalWrite(4,HIGH);
  pinMode(5,OUTPUT);
  pinMode(6,OUTPUT);
  pinMode(7,INPUT);
  digitalWrite(5,LOW);
 //definiujemy połączenia z Arduino do poszczególnych portów 
}
 
void loop() {

  if(z==false && v==false && Serial.available())
{ 
  dlugosc=Serial.parseInt();
  digitalWrite(5,HIGH);
  delay(1000);
  digitalWrite(5,LOW);
z=true;
v=true;

}
if(v==true && d==false && Serial.available())
{
  wysokosc=Serial.parseInt();
  digitalWrite(5,HIGH);
  delay(1000);
  digitalWrite(5,LOW);
v=false;
d=true;

}
if(d==true && z==true && Serial.available())
{
 kacik=Serial.parseInt();
 digitalWrite(5,HIGH);
  delay(1000);
  digitalWrite(5,LOW);

d=false;
z=true;
}//odbieramy dane z pierwszego arduino poprzez połączenie portem serial
Serial.println(dlugosc);
Serial.println(wysokosc);
Serial.println(kacik);
//wyświetlamy wartości na konsoli aby sprawdzić przesył danych
if(dlugosc!=0 && wysokosc!=0 && kacik!=0)
{
  
  
  digitalWrite(6, LOW);
  delayMicroseconds(2);
  digitalWrite(6, HIGH);
  delayMicroseconds(10);
  digitalWrite(6, LOW);
 
  czas = pulseIn(7, HIGH);
  dystans = czas / 58;
  //definiujemy rzeczy potrzebne do działania czujnika ultradźwiękowego
  if(dystans<=9)//sprawdzamy czujnikiem ultradźwiękowym czy pojawił się znak wykonania sekwencji
  {
    digitalWrite(12,HIGH);
    int i = S2.read();
    int j = S1.read();
    int nom=S3.read();//czytame bieżące pozycje poszczególnych serv
  			
   float katServ[2];
   kroki1(dlugosc,kacik);//uruchamiamy funkcję, która na podstawie współrzędnych x i y liczy odpowiedni kąt obrotu oraz długość potrzebną dalej 
   if(nom<80){
     for(nom;nom<=80;nom++)
        {
        S3.write(nom);
        delay(30);
        } //koniec fora 
        }//rozwieramy chwytak
	if(prze!=przeciw && wysoko!=wysokosc)
	 {      
	  check(przeciw, wysokosc, katServ);//uruchamiamy funkcję liczącą optymalne położenie dla serv 1 i 2
	  prze=przeciw;
	  wysoko=wysokosc;
	 }

    idzie(katServ[0],katServ[1],30);//uruchamiamy funkcję która porusza servami w odpowiednim tempie
     delay(500);
     if(nom>54){
     for(nom;nom>=54;nom--)
        {
        S3.write(nom);
        delay(30);
        } //koniec fora 
        } //zamykamy chwytak
        delay(1000);
         wraca(120,30,30);//uruchamiamy funkcję która porusza servami w odpowiednim tempie
    delay(1000);
while(stepper.currentPosition() != 0)
  {
    if(kroki>=0)
    {
     stepper.setSpeed(-200); 
    }
    else
    {
      stepper.setSpeed(200);
    }
      
    stepper.runSpeed();
  }//wracamy do położenia początkowego
  delay(1000);
if(nom<80){
     for(nom;nom<=80;nom++)
        {
        S3.write(nom);
        delay(30);
        } //koniec fora 
        }//rozwieramy chwytak
    digitalWrite(12,LOW);
    }//przycisk
}//koniel loopa
   
 }
void idzie(float pierwszy, float drugi, int szybkosc)
{
int i = S2.read();
int j = S1.read();
if(i<drugi){
     for(i;i<=drugi;i++)
        {
        S2.write(i);
        delay(szybkosc);
        } //koniec fora 
        }
    if(i>drugi){
     for(i;i>=drugi;i--)
        {
        S2.write(i);
        delay(szybkosc);
        } //koniec fora 
        }
    if(j<pierwszy){
    for(j;j<=pierwszy;j++)
        {
        S1.write(j);
        delay(szybkosc);
        } //koniec fora
        }
    if(j>pierwszy){
    for(j;j>=pierwszy;j--)
        {
        S1.write(j);
        delay(szybkosc);
        } //koniec fora
        }

//funkcja dostosowuje ruch do optymalnych współrzędnych do zadanego czasu oraz wybiera w którą stronę powinna się poruszać
}//koniec idzie

void wraca(float pierwszy, float drugi, int szybkosc)
{
int i = S2.read();
int j = S1.read();

    if(j<pierwszy){
    for(j;j<=pierwszy;j++)
        {
        S1.write(j);
        delay(szybkosc);
        } //koniec fora
        }
    if(j>pierwszy){
    for(j;j>=pierwszy;j--)
        {
        S1.write(j);
        delay(szybkosc);
        } //koniec fora
        }

  if(i<drugi){
     for(i;i<=drugi;i++)
        {
        S2.write(i);
        delay(szybkosc);
        } //koniec fora 
        }
    if(i>drugi){
     for(i;i>=drugi;i--)
        {
        S2.write(i);
        delay(szybkosc);
        } //koniec fora 
        }
//funkcja dostosowuje ruch do optymalnych współrzędnych do zadanego czasu oraz wybiera w którą stronę powinna się poruszać działa w odwrotnej kolejności w porównaniu do funkcji idzie

}//koniec wraca

void kroki1(float x,float y) 
{
Serial.println(x);
Serial.println(y);
float kat;
float katy;
float male=23;
float x1;
float y1;
 digitalWrite(13,LOW); //włączenie zasilania na silniku (przeciwnie do wskazówek zegara)

 
   stepper.setCurrentPosition(0);
   x1=abs(x);
   y1=abs(y);
   przeciw=sqrt(x*x+y*y);

   if(x>0 && y<0){
        kat = atan(y1/x1 + male/przeciw);
  katy=(kat*180)/PI;
        kroki = (katy*800/360);
   }
    if(x<0 && y<0){
        kat = atan(x1/y1 + male/przeciw);//sprawdzic
  katy=(kat*180)/PI;
        kroki =200 + katy*800/360;
   }
   if(x>0 && y>0){
        kat = atan(y1/x1 - male/przeciw);
  katy=(kat*180)/PI;
        kroki = -(katy*800/360);
   }
    if(x<0 && y>0){
        kat = atan(x1/y1 - male/przeciw);//sprawdzic
  katy=(kat*180)/PI;
        kroki = -(katy*800/360)-200;
   }
   Serial.println(katy);
   Serial.println(kroki);
 while(stepper.currentPosition() != kroki)
  {
    if(kroki>=0)
    {
      stepper.setSpeed(200);
    }
    else
    {
      stepper.setSpeed(-200);
    }
    stepper.runSpeed();
  }
  
 digitalWrite(13,HIGH);//wyłączenie zasilania na silniku
  delay(1000);
//funkcja liczy odpowiedni kąt i porusza o ten kąt silnik krokowy liczy również wartość potrzebną do funkcji check
}//koniec kroki1


void check(float odlx,float wysy,float katServ[])
{
    int i,j;
    float h,X,D,y1,y2,kat1,kat2,odl,roznica,alfa,beta,odwrot;
    roznica=100;
    

    for (i=S1min;i<=S1max;i++)
    {
        for (j=S2min; j<=S2max;j++)
        {   
          odwrot=180-j;
            alfa=abs(i-90);
            if(i>=90)
            {   if((j+alfa)<180)
                beta=abs((odwrot-90+alfa));
                else if(i+j==270)
                beta=0;
                else
                beta=odwrot-180+alfa;
                
            }
            else
            {   
                if(odwrot<=alfa)
                beta=abs(alfa-odwrot);
                else
                beta=abs((odwrot-90-alfa));
                
            }
          
       
      kat1= alfa/180*PI;//kąt w radianach
            kat2= beta/180*PI;
          
            if(((i>=90) && ((odwrot+alfa)>=180)) || ((i<90) && (odwrot<=alfa)))
              {
                D=cos(kat2)*c;
                y2=sin(kat2)*c;
                X=cos(kat1)*b;
                y1=sin(kat1)*b;
              }
            else
              {
                X=cos(kat1)*b;
                D=sin(kat2)*c;
                y1=sin(kat1)*b;
                y2=cos(kat2)*c;
              }

            if((i>=90) && ((odwrot+alfa)>=180))
            h=a+y1+y2-e;
            else if((i>=90) && ((odwrot+alfa)<180))
            h=a+y1-y2-e;
            else if((i<90) && (odwrot<=alfa))
            h=a-y1+y2-e;
            else if((i<90) && (odwrot>alfa))
            h=a-y1-y2-e;

            if(((odwrot>=(90-alfa)) && (i>=90)) || ((i<90) &&  (odwrot>=(90+alfa))))
            odl=X+D;
            else
            odl=X-D;

            
            if ((roznica>=(abs(odlx-odl)+abs(wysy-h))) && (wysy<=h))
            {
                roznica=(abs(odlx-odl)+abs(wysy-h));//wazne
                katServ[0]=i+10; //nieścisłość kątów na prototypie
                katServ[1]=j; 
               
            }

        }
    }
//funkcja szukająca optymalnego położenia dla zadanych odl od środka obrotu  na podstawie analizy ruchu 3 ruchomych części
}//koniec check